#include "stdint-gcc.h"
#include "pid_config.h"
#include "stdio.h"
#include "math.h"

volatile uint32_t gPositionPID_TimeCNT;
volatile uint32_t gSpeedPID_TimeCNT;

/*----------------------------------------------------------------------------*/
/*-------------------------      current pid     -----------------------------*/
/*----------------------------------------------------------------------------*/

/*************************************************************
** Function name:       FOCPrintf
** Descriptions:        FOC打印信息
** Input parameters:    None
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void FOCPrintf(PFOC_Struct pFOC)
{
    printf("1:%f\r\n", pFOC->ia);
    printf("2:%f\r\n", pFOC->ib);
    printf("3:%f\r\n", pFOC->ic);
    printf("5:%f\r\n", pFOC->id);
    printf("6:%f\r\n", pFOC->iq);
    // printf("7:%f\r\n",pFOC->tarid);
    printf("7:%f\r\n", pFOC->idPID.out);
    printf("8:%f\r\n", pFOC->iqPID.out);
}

/*************************************************************
** Function name:       SetCurrentPIDTar
** Descriptions:        设置D轴和Q轴的目标值
** Input parameters:    pFOC：FOC对象指针
**                      tarid：D轴目标电流
**                      tariq：Q轴目标电流
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetCurrentPIDTar(PFOC_Struct pFOC, float tarid, float tariq)
{
    pFOC->tarid = tarid;
    pFOC->tariq = tariq;
}


/*************************************************************
** Function name:       CurrentPIControlID
** Descriptions:        D轴电流闭环
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void CurrentPIControlID(PFOC_Struct pFOC)
{
    // 获取实际值
    pFOC->idPID.pre = pFOC->id;
    // 获取目标值
    pFOC->idPID.tar = pFOC->tarid;
    // 计算偏差
    pFOC->idPID.bias = pFOC->idPID.tar - pFOC->idPID.pre;
    // 计算PID输出值
    pFOC->idPID.out += pFOC->idPID.kp * (pFOC->idPID.bias - pFOC->idPID.lastBias) + pFOC->idPID.ki * pFOC->idPID.bias;
    // 保存偏差
    pFOC->idPID.lastBias = pFOC->idPID.bias;

    if (pFOC->idPID.out > fabs(pFOC->idPID.outMax))
    {
        pFOC->idPID.out = fabs(pFOC->idPID.outMax);
    }

    if (pFOC->idPID.out < -fabs(pFOC->idPID.outMax))
    {
        pFOC->idPID.out = -fabs(pFOC->idPID.outMax);
    }
}

/*************************************************************
** Function name:       CurrentPIControlIQ
** Descriptions:        Q轴电流闭环
** Input parameters:    pFOC:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void CurrentPIControlIQ(PFOC_Struct pFOC)
{
    // 获取实际值
    pFOC->iqPID.pre = pFOC->iq;
    // 获取目标值
    pFOC->iqPID.tar = pFOC->tariq;
    // 计算偏差
    pFOC->iqPID.bias = pFOC->iqPID.tar - pFOC->iqPID.pre;
    // 计算PID输出值
    pFOC->iqPID.out += pFOC->iqPID.kp * (pFOC->iqPID.bias - pFOC->iqPID.lastBias) + pFOC->iqPID.ki * pFOC->iqPID.bias;
    // 保存偏差
    pFOC->iqPID.lastBias = pFOC->iqPID.bias;

    if (pFOC->iqPID.out > fabs(pFOC->iqPID.outMax))
    {
        pFOC->iqPID.out = fabs(pFOC->iqPID.outMax);
    }

    if (pFOC->iqPID.out < -fabs(pFOC->iqPID.outMax))
    {
        pFOC->iqPID.out = -fabs(pFOC->iqPID.outMax);
    }
}

/*----------------------------------------------------------------------------*/
/*-------------------------       speed pid      -----------------------------*/
/*----------------------------------------------------------------------------*/

/*************************************************************
** Function name:       SpeedPIDControl
** Descriptions:        速度PID控制
** Input parameters:    pSpeedPID:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
static void SpeedPIDControl(PSpeedPIDControl_Struct pSpeedPID)
{

    pSpeedPID->sPID.pre = pSpeedPID->GetPreSpeed();
    pSpeedPID->sPID.bais = pSpeedPID->sPID.tar - pSpeedPID->sPID.pre;
    pSpeedPID->sPID.out += pSpeedPID->sPID.kp * (pSpeedPID->sPID.bais - pSpeedPID->sPID.last_bias) + pSpeedPID->sPID.ki * pSpeedPID->sPID.bais;
    pSpeedPID->sPID.last_bias = pSpeedPID->sPID.bais;

    if (pSpeedPID->sPID.out > pSpeedPID->sPID.outMax)
    {
        pSpeedPID->sPID.out = pSpeedPID->sPID.outMax;
    }
    else if (pSpeedPID->sPID.out < -pSpeedPID->sPID.outMax)
    {
        pSpeedPID->sPID.out = -pSpeedPID->sPID.outMax;
    }
    if (pSpeedPID->isEnable == 0)
    {
        pSpeedPID->sPID.out = 0;
    }
    pSpeedPID->SetOutCurrent(pSpeedPID->sPID.out);
}

/*************************************************************
** Function name:       SpeedPIDCycle
** Descriptions:        速度PID循环函数
** Input parameters:    pSpeedPID:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SpeedPIDCycle(PSpeedPIDControl_Struct pSpeedPID)
{
    if (SPEEDPID_TIMEOUT(pSpeedPID->sPID.cycle * 1000.0f, pSpeedPID->sysTimer))
    {
        pSpeedPID->sysTimer = SPEEDPID_GETTIME();
        SpeedPIDControl(pSpeedPID);
    }
}

/*************************************************************
** Function name:       SetSpeedPIDParams
** Descriptions:        设置速度PID参数
** Input parameters:    pSpeedPID:结构体指针
**                      kp:比例
**                      ki:积分
**                      kd:微分
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetSpeedPIDParams(PSpeedPIDControl_Struct pSpeedPID, float kp, float ki, float kd)
{
    pSpeedPID->sPID.kp = kp;
    pSpeedPID->sPID.ki = ki;
    pSpeedPID->sPID.kd = kd;
}

/*************************************************************
** Function name:       SetSpeedPIDTar
** Descriptions:        设置目标值
** Input parameters:    pSpeedPID:结构体指针
**                      tar:目标速度
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetSpeedPIDTar(PSpeedPIDControl_Struct pSpeedPID, float tar)
{
    pSpeedPID->sPID.tar = tar;
}

/*************************************************************
** Function name:       SetSpeedPIDEnable
** Descriptions:        设置速度PID使能
** Input parameters:    pSpeedPID:结构体指针
**                      isEnable:1: 使能0：失能
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SetSpeedPIDEnable(PSpeedPIDControl_Struct pSpeedPID, float isEnable)
{
    pSpeedPID->isEnable = isEnable;
}

/*************************************************************
** Function name:       SpeedPIDPrintf
** Descriptions:        速度PID信息打印
** Input parameters:    pSpeedPID:结构体指针
** Output parameters:   None
** Returned value:      None
** Remarks:             None
*************************************************************/
void SpeedPIDPrintf(PSpeedPIDControl_Struct pSpeedPID)
{
    printf("2:%f\r\n", pSpeedPID->sPID.pre);
    printf("3:%f\r\n", pSpeedPID->sPID.tar);
}





